classdef MotionModel < matlab.System
    % 运动模型: 按照配置参数，完成:
    % 1. 几何模型 中   坐标系的定义和转换
    % 2. 卫星相对天线的俯仰角， 方位角的计算
    
    properties(Nontunable)
        config              % 信号生成配置
        numAntennas         % 天线数量（M=4）
        rotationRadius      % 旋转半径(m)
        initialRotationPhase% 初始旋转相位(rad)
        motionUpdateTime    % 运动更新时间
    end

    properties(Access = private)
        antennaAngles       % 初始天线角度（均匀分布：2π(m-1)/M）
        prevTime            % 上一段数据的结束时间（确保时间连续性）
        prevRotationFreq    % 上一段结束时的旋转频率（确保动态连续）
        % currentStep = 1     % 当前读取precomputedUS的步骤索引
    end

    properties(Access = public)
        carrierCoor         % 载体坐标系（3×3，固定不变）
        antennaRadiationCoor   % 天线辐射坐标系（M×3×3，向量化存储）
        rotationFreq        % 旋转频率(Hz)
        rotationFreqRate    % 旋转频率变化率(Hz/s)
        precomputedUS       % 预计算的卫星位置单位向量序列（N×3矩阵）
    end

    methods(Access = public)
        function obj = MotionModel(config, ~)
            % 初始化：加载流处理运动参数
            obj.config = config;
            obj.numAntennas = config.antennas.count;
            obj.rotationRadius = config.rotation.radius;
            obj.initialRotationPhase = config.rotation.initialPhase;
            obj.rotationFreq = config.rotation.freq;
            obj.rotationFreqRate = config.rotation.freqRate;
            
            % 1. 运动更新时间（流处理分段基础）
            obj.motionUpdateTime = config.motion.motionUpdateTime; 
            
            % 2. 预计算天线初始角度（M×1向量，向量化）
            obj.antennaAngles = 2 * pi * (0 : obj.numAntennas - 1)' / obj.numAntennas;
            
            % 3. 流处理初始状态
            obj.prevTime = 0;
            obj.prevRotationFreq = obj.rotationFreq; % 初始旋转频率

            % 4. 定义载体坐标系（3×3，固定不变）
            uD = [1,0,0]; % 载体x轴（正东）
            uL = [0,1,0]; % 载体y轴（正北）
            uH = [0,0,1]; % 载体z轴（天顶，旋转轴）
            obj.carrierCoor = [uD; uL; uH]; 
            
            % 初始化天线辐射坐标系（M×3×3，初始与载体坐标系一致）
            obj.antennaRadiationCoor = repmat(obj.carrierCoor, [1, 1, obj.numAntennas]);

            % 5. precomputedUS 序列产生并验证
            precomputedUS = obj.generatePrecomputedUS(config);

            if size(precomputedUS, 2) ~= 4
                error('precomputedUS必须是N×4矩阵（每行对应一个时间步的[x,y,z,t]）');
            end
            obj.precomputedUS = precomputedUS;
        end

    end

    methods(Access = protected)
        function setupImpl(obj)
            % 初始化参数验证（流处理前确保合法性）
            if obj.rotationRadius <= 0
                error('旋转半径必须为正数（文档1.1节几何模型要求）');
            end
            if obj.motionUpdateTime <= 0
                error('运动更新时间必须为正数（流处理分段基础）');
            end
        end

        function motionDataSeg = stepImpl(obj, t)
            % 按照输入时间t，生成单段运动数据
            % 输入：t - 当前时刻（单个数字或向量），可以有t，也可以没有t
            % 输出：motionDataSeg - 单段运动数据结构体
            %       - time：单段时间
            %       - carrierCoor：1×3，单段载体坐标系位置
            %       - antennaRadiationCoor：M×3，单段天线辐射坐标系
            %       - thetaM：卫星相对于所有天线的俯仰角（弧度）   
            %       - phiM：卫星相对于所有天线的方位角（弧度）
            %       - cosThetaM：卫星相对于所有天线的俯仰角的余弦值
            %       - cosPhiM：卫星相对于所有天线的方位角的余弦值
   

            % 警告: 如果t向量首尾时间差距大于运动更新时间，可能会导致误差
            if abs(t(end) - t(1)) > obj.motionUpdateTime
                warning('输入时间向量首尾时间差距大于运动更新时间，可能会导致误差');
            end
            
            % 1. 生成当前时刻，如果t为向量，需要先转变为单个数字
            if ~isscalar(t)
                currentTime = t(1);
            else
                currentTime = t;
            end

            % 2. 计算当前旋转频率（考虑变化率）
            currentRotationFreq = obj.prevRotationFreq + obj.rotationFreqRate * (currentTime - obj.prevTime);
            obj.prevRotationFreq = currentRotationFreq;
            
            % 3. 向量化计算所有天线的旋转角度ψₘ(t)（M×1向量）
            % 公式：ψₘ(t) = 2π(f_R(t)t + m_idx/M) + ψ_R，m_idx=0~M-1
            m_idx = (0 : obj.numAntennas - 1)'; % 天线索引（M×1）
            psiM = 2 * pi * (currentRotationFreq * currentTime + m_idx / obj.numAntennas) + ...
                   obj.initialRotationPhase;
            % psiM = mod(psiM, 2*pi); % 可选：归一化到0~2π
            
            % 4. 按照时间t读取当前卫星位置向量u_S（1×3）
            % 查找t在precomputedUS中的索引（假设t单调递增）
            idx = find(obj.precomputedUS(:, 4) <= currentTime, 1, 'last'); 
            if isempty(idx)
                error('当前时间%.3fs早于预计算起始时间%.3fs', currentTime, obj.precomputedUS(1,4));
            end
            uS = obj.precomputedUS(idx, 1:3); % 提取当前位置向量（x,y,z）
            
            % ---------------------- 向量化计算所有天线的核心参数 ----------------------
            % 5. 向量化计算所有天线的方向向量uM（M×3）和横向轴uTm（M×3）
            cos_psi = cos(psiM); % M×1
            sin_psi = sin(psiM); % M×1
            % uM = cosψ*uD + sinψ*uL（载体x/y轴线性组合，向量化）
            uM = cos_psi * obj.carrierCoor(1, :) + sin_psi * obj.carrierCoor(2, :); 
            % uTm = cosψ*uL - sinψ*uD（横向轴，向量化）
            uTm = cos_psi * obj.carrierCoor(2, :) - sin_psi * obj.carrierCoor(1, :); 
            
            % 6. 向量化计算所有天线的辐射坐标系（M×3×3）
            % 每个天线的坐标系：[uM(m,:); uTm(m,:); uH]，通过repmat和矩阵拼接实现向量化
            uH_rep = repmat(obj.carrierCoor(3, :), [obj.numAntennas, 1]); % M×3（z轴重复）
            antennaRadiationCoor_2d = [uM; uTm; uH_rep]; % 3M×3（按行堆叠所有轴）
            % 重塑为M×3×3：每个天线对应3行（x/y/z轴）
            obj.antennaRadiationCoor = reshape(permute(reshape(antennaRadiationCoor_2d, 3, [], obj.numAntennas), [2,1,3]), 3, 3, obj.numAntennas);
            obj.antennaRadiationCoor = permute(obj.antennaRadiationCoor, [3,1,2]); % 最终M×3×3
            
            % 7. 向量化计算所有天线的俯仰角θₘ（M×1）及余弦值（M×1）
            % cosThetaM = uM * uS'; % M×3 * 3×1 = M×1（内积向量化），这里存在问题，我们需要计算从uM到uS的角度，是一个有方向的角度，直接计算内积是没有方向的
            % thetaM = acos(cosThetaM); % M×1，修正为从0到π的角度

            % 下面这四行代码说明了内积计算下的具体展开，与公式 cosThetaM = uS * obj.carrierCoor(1,:)' * cos(psiM) + uS * obj.carrierCoor(2,:)' * sin(psiM); 是等价的
            % cosThetaM = uS * obj.carrierCoor(1,:)' * cos(psiM) + uS * obj.carrierCoor(2,:)' * sin(psiM);
            % if cosThetaM ~= uM * uS'
            %     error('计算俯仰角时出现错误，可能是输入向量uM或uS存在问题');
            % end
            % thetaM = acos(cosThetaM); % M×1

            r_vec = uS - uM;  % 必须是卫星 - 天线位置！！！
            cosThetaM = r_vec(:,3) ./ vecnorm(r_vec, 2, 2);               % cosθ_m
            sinThetaM = sqrt(1 - cosThetaM.^2);                           % |sinθ_m|
            thetaM = acos(cosThetaM);                 % M×1

            % 8. 向量化计算所有天线的方位角φₘ（M×1）及余弦值（M×1）
            cosPhiM   = dot(r_vec, uTm, 2) ./ (vecnorm(r_vec,2,2) .* abs(sinThetaM));
            phiM      = acos(cosPhiM);       % 方位角 φ_m(t) 

            % 平行投影：Pa_uM = (uS·uM)*uM（M×3，向量化）
            % Pa_uM = (cosThetaM .* uM); % 利用uS·uM=cosThetaM，避免重复内积
            % 垂直投影：Pperp_uM_uS = uS - Pa_uM（M×3，向量化）
            % Pperp_uM_uS = repmat(uS, [obj.numAntennas, 1]) - Pa_uM; 
            % 计算垂直投影的模（M×1，向量化）
            % norm_Pperp = sqrt(sum(Pperp_uM_uS.^2, 2)); 
            % 处理模为0的情况（共线），避免除以零
            % norm_Pperp(norm_Pperp < 1e-6) = 1e-6; 
            % 方位角余弦值（M×1，向量化：内积/模）
            % cosPhiM = sum(Pperp_uM_uS .* uTm, 2) ./ norm_Pperp; 
            % 基础方位角（0~π，M×1）
            % phiM = acos(cosPhiM); 
            % 向量化判断方位角符号（扩展到-π~π）：叉积z分量符号
            % cross_z = sum(Pperp_uM_uS .* repmat(cross(obj.carrierCoor(1,:), obj.carrierCoor(2,:)), [obj.numAntennas, 1]), 2);
            % phiM = phiM_base .* sign(cross_z); % 最终方位角（M×1）
            % --------------------------------------------------------------------------
            
            % 9. 封装所有天线的输出数据
            motionDataSeg.time = currentTime;
            motionDataSeg.rotationFreq = currentRotationFreq;
            motionDataSeg.carrierCoor = obj.carrierCoor; % 载体坐标系不变（3×3）
            motionDataSeg.antennaRadiationCoor = obj.antennaRadiationCoor; % M×3×3
            motionDataSeg.cosThetaM = cosThetaM; % M×1（俯仰角余弦）
            motionDataSeg.cosPhiM = cosPhiM;     % M×1（方位角余弦）
            motionDataSeg.thetaM = thetaM;               % M×1（俯仰角，弧度）
            motionDataSeg.phiM = phiM;                   % M×1（方位角，弧度）

            % 10. 更新时间状态
            obj.prevTime = currentTime;
        end

        function resetImpl(obj)
            % 重置流处理状态（重新开始仿真时调用）
            obj.prevTime = 0; 
            obj.prevRotationFreq = obj.rotationFreq;
            % obj.currentStep = 1;
        end
    end

    methods(Access = public)
        function x = clamp(obj, x, minVal, maxVal)
            % 限制数值范围，避免acos输入超界
            x(x < minVal) = minVal;
            x(x > maxVal) = maxVal;
        end

        function precomputedUS = generatePrecomputedUS(obj, config)  
            % 基于GPS卫星过境场景构建卫星相对旋转载体旋转中心的俯仰角和方位角变化，从而计算u_S(t),也就是卫星相对于旋转中心的位置向量
            % 生成预计算的u_S序列（文档1.1节u_S(t)）

            % 1. 从配置中提取参数
            simDuration = config.sim.duration;
            simStartTime = 0;
            
            % 2. 校验过境时间
            passTime = config.motion.passTime;
            if passTime < simDuration
                error('卫星过境时间(passTime=%ds)小于仿真总时长(sim.duration=%ds)', passTime, simDuration);
            end
            
            % 3. 计算总步数
            N = ceil(simDuration / obj.motionUpdateTime);
            precomputedUS = zeros(N + 1, 4);
            
            % 4. 时间轴（包含起始时间）
            t = (0 : N) * obj.motionUpdateTime;
            
            % 卫星过境场景下卫星相对于旋转中心的俯仰角和方位角变化
            elevation = getElevationOrAzimuth(config.motion.elevMax, config.motion.elevMin, passTime, t, config.motion.useSineForElevation);
            azimuth = getElevationOrAzimuth(config.motion.azEnd, config.motion.azStart, passTime, t, config.motion.useSineForAzimuth);

            % 打印俯仰角和方位角的变化
            fprintf('时间变化范围(s): %.4f ~ %.4f\n', min(t), max(t));
            fprintf('卫星-旋转中心的俯仰角变化（rad）: %.4f ~ %.4f\n', min(elevation), max(elevation));
            fprintf('卫星-旋转中心的方位角变化（rad）: %.4f ~ %.4f\n', min(azimuth), max(azimuth));
            
            % 6. 计算u_S向量
            xyProj = cos(elevation);
            precomputedUS(:, 1) = xyProj .* cos(azimuth);
            precomputedUS(:, 2) = xyProj .* sin(azimuth);
            precomputedUS(:, 3) = sin(elevation);
            precomputedUS(:, 4) = t;
        end
    end
end

